function [ phi, theta, psi, psiTrue ] = dcm2rollpityawhead( CBL, alpha, rollOnSingu ) %#codegen
%DCM2ROLLPITYAWHEAD 将方向余弦矩阵和游移方位角转换为滚动、俯仰、偏航和真北方位角
%
% Tips:
% # L系初始为NED
%
% Input Arguments:
% # CBL: 3*3*m矩阵，方向余弦矩阵
% # alpha: 长度为m的列向量，游移方位角，导航系从初始姿态绕天向转动（按右手定则）的角度，默认全为0，单位rad
% # rollOnSingu: 标量，当俯仰角处于奇异区间（-pi/2或pi/2附近）时默认的滚动角，单位rad
%
% Output Arguments:
% # phi: 长度为m的列向量，滚动角，绕X轴，值域(-pi, pi]，单位rad
% # theta: 长度为m的列向量，俯仰角，绕Y轴，值域[-pi/2, pi/2]，单位rad
% # psi: 长度为m的列向量，偏航角，绕Z轴，值域(-pi, pi]，单位rad
% # psiTrue: 长度为m的列向量，真北方位角，由北向起顺时针为正，值域[0, 2*pi)，单位rad
%
% References
% #《应用导航算法工程基础》“方向余弦矩阵转换为欧拉角”

if nargin < 2
    alpha = zeros(size(CBL, 3), 1);
end
if nargin < 3
    rollOnSingu = 0;
end

p = size(CBL, 3);
[psi, theta, phi] = dcm2angle_ont(CBL, rollOnSingu);
psiTrue = NaN(p, 1);
for i=1:p
      psiTrue(i, 1) = psi(i, 1) - alpha(i, 1);
      phi(i, 1) = mod(phi(i, 1), 2*pi);
    if phi(i, 1) > pi
        phi(i, 1) = phi(i, 1) - 2*pi;
    end
    psi(i, 1) = mod(psi(i, 1), 2*pi);
    if psi(i, 1) > pi
        psi(i, 1) = psi(i, 1) - 2*pi;
    end
    psiTrue(i, 1) = mod(psiTrue(i, 1), 2*pi);
end
end